#!/usr/bin/python
# coding=utf-8
import os
import rospy
import tf
import time
import sys
import math
import serial
import string
from std_msgs.msg import Int8
from std_msgs.msg import String
import ctypes

#class queue is design for uart receive data cache
class queue:
    def __init__(self, capacity = 1024*4):
        self.capacity = capacity
        self.size = 0
        self.front = 0
        self.rear = 0
        self.array = [0]*capacity
 
    def is_empty(self):
        return 0 == self.size
 
    def is_full(self):
        return self.size == self.capacity
 
    def enqueue(self, element):
        if self.is_full():
            raise Exception('queue is full')
        self.array[self.rear] = element
        self.size += 1
        self.rear = (self.rear + 1) % self.capacity
 
    def dequeue(self):
        if self.is_empty():
            raise Exception('queue is empty')
        self.size -= 1
        self.front = (self.front + 1) % self.capacity
 
    def get_front(self):
        return self.array[self.front]
    
    def get_front_second(self):
        return self.array[((self.front + 1) % self.capacity)]

    def get_queue_length(self):
        return (self.rear - self.front + self.capacity) % self.capacity

    def show_queue(self):
        for i in range(self.capacity):
            print self.array[i],
        print(' ')

#class BarcodeControl:
class BarcodeControl:
    def __init__(self):
        #Get params
        self.baseId = rospy.get_param('~base_id','base_footprint')
        self.device_port = rospy.get_param('~barcode_port','/dev/barcode')
        self.baudrate = int(rospy.get_param('~baudrate','115200'))
        self.freq = int(rospy.get_param('~freq','20'))
        self.barcode_topic = rospy.get_param('~barcode_topic','/barcode')


        #define param
        self.current_time = rospy.Time.now()
        self.previous_time = self.current_time
        self.last_cmd_vel_time = rospy.Time.now()
        self.Circleloop = queue(capacity = 1024 *4)
        # Serial Communication
        try:
            self.serial = serial.Serial(self.device_port,self.baudrate,timeout=10)
            # rospy.loginfo("Opening Serial")
            try:
                if self.serial.in_waiting:
                    self.serial.readall()
            except:
                # rospy.loginfo("Opening Serial Try Faild")
                pass
        except:
            rospy.logerr("Can not open Serial"+self.device_port)
            self.serial.close
            sys.exit(0)
        # rospy.loginfo("Serial Open Succeed")
        #if move base type is ackermann car like robot and use ackermann msg ,sud ackermann topic,else sub cmd_vel topic
        self.barcode_pub = rospy.Publisher(self.barcode_topic,String,queue_size=10)
        self.timer_communication = rospy.Timer(rospy.Duration(1.0/20),self.timerCommunicationCB)

    #Communication Timer callback to handle receive data
    #depend on communication protocol
    def timerCommunicationCB(self,event):
        # Receive Information
        length = self.serial.in_waiting
        if length:
            data = self.serial.read_all()
<<<<<<< HEAD
            if isinstance(data,str) and len(data)>1:
                self.barcode_pub.publish(data)
            # rospy.loginfo("Read Barcode Content :%s" % data)
=======
            if isinstance(data,basestring):
                self.barcode_pub.publish(data)
                rospy.loginfo("Read Barcode Content :%s" % data)
>>>>>>> 98bb63be115a2f60051d73e737d5902d0b27b687
        #     try:
        #         self.Circleloop.enqueue(data)
        #     except:
        #         rospy.logerr("Circleloop.enqueue Faild")
        # else:
        #     pass
        # if self.Circleloop.is_empty()==False:
        #     if self.Circleloop.is_empty()==False:
        #         data = self.Circleloop.get_front()
        #         rospy.loginfo("Read Barcode Content 2:%s" % data)
        #         self.barcode_pub.publish(data)
        #     else:
        #         pass
        # else:
            # rospy.loginfo("Circle is Empty")
            # pass
        # rospy.loginfo("self.clean_report_status:%s" %self.clean_report_status)
        
#main functionc'
if __name__=="__main__":
    try:
        rospy.init_node('clean_control',anonymous=True)
        bc = BarcodeControl()
        rospy.spin()
    except KeyboardInterrupt:
        bc.serial.close
        print("Shutting down")
